/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.SmartDashboard;

/**
 *
 * @author robotics
 */
public class DriveQuadEncoder implements PIDSource
{
    DigitalInput inputA;
    DigitalInput inputB;
    Encoder encoder;
    double prevDist;
    double currDist;
    double prevTime = 0;
    double maxRate = 0;
    int direction = 1;
    boolean lastDirection = true;
    String encoderName;

    public DriveQuadEncoder(int achannel, int bchannel, String name)
    {
        encoderName = name;
        inputA = new DigitalInput(achannel);
        inputB = new DigitalInput(bchannel);
        encoder = new Encoder(inputA, inputB);
        encoder.setDistancePerPulse(Constants.DISTANCE_PER_PULSE);
        encoder.start();
        prevDist = 0;
        currDist = 0;
    }

    public double pidGet()
    {
        double currTime = System.currentTimeMillis();
        prevTime = currTime;

        currDist = encoder.get();

        double rate = currDist - prevDist;

        SmartDashboard.log(rate, this.encoderName + " encoder");

        boolean currentDirection = encoder.getDirection();
 //       SmartDashboard.log(currentDirection, "currentDirection");
//        if (currentDirection == false) {
//            direction = direction * -1;
//        }
 //       SmartDashboard.log(rate, "Rate");
//        double rate = distance;
        
        rate = Utils.limitValue(rate, Constants.MAX_RATE);

        //encoder.reset();
        prevDist = currDist;

        return rate/Constants.MAX_RATE;
    }

    public double getDistance()
    {
        return encoder.getDistance();
    }

}
